#pragma once

#include "../arch/avx.h"
#include "vec2i.h"
#include <math.h>

OPEN_JLIB_NS


struct Vec2 {

  f64 p;

  inline Vec2 () noexcept {
  }
  inline Vec2 (Vec2i v) noexcept {
    p = avx_vcvtdq2ps<f64>(v.p);
  }
  inline Vec2 (f64 p) noexcept : p(p) {
  }
  inline Vec2 (f32 x, f32 y) noexcept {
    p = avx_vinsertps<0x1C, f64>(x, y);
  }
  inline Vec2 (f32 s) noexcept {
    p = avx_vshufps<0x00, f64>(s, s);
  }
  inline Vec2 (const f32* m) noexcept {
    p = avx_vmovq<f64>(m);
  }


  inline f32 x () const noexcept {
    return avx_cast<f32>(p);
  }
  inline f32 y () const noexcept {
    return avx_vinsertps<0x4E, f32>(p, p);
  }


  inline Vec2& x (f32 s) noexcept {
    p = avx_vinsertps<0x0C, f64>(p, s);
    return *this;
  }
  inline Vec2& y (f32 s) noexcept {
    p = avx_vinsertps<0x1C, f64>(p, s);
    return *this;
  }


  inline Vec2& operator+= (Vec2 v) noexcept {
    p = avx_vaddps<f64>(p, v.p);
    return *this;
  }
  inline Vec2& operator-= (Vec2 v) noexcept {
    p = avx_vsubps<f64>(p, v.p);
    return *this;
  }
  inline Vec2& operator*= (Vec2 v) noexcept {
    p = avx_vmulps<f64>(p, v.p);
    return *this;
  }
  inline Vec2& operator/= (Vec2 v) noexcept {
    p = avx_vdivps<f64>(p, v.p);
    return *this;
  }


  inline Vec2& operator*= (f32 s) noexcept {
    auto s2 = avx_vshufps<0x00, f64>(s, s);
    p = avx_vmulps<f64>(p, s2);
    return *this;
  }
  inline Vec2& operator/= (f32 s) noexcept {
    auto s2 = avx_vshufps<0x00, f64>(s, s);
    p = avx_vdivps<f64>(p, s2);
    return *this;
  }


  inline Vec2& clamp (Vec2 min, Vec2 max) noexcept {
    p = avx_vmaxps<f64>(p, min);
    p = avx_vminps<f64>(p, max);
    return *this;
  }
  inline f32 normalize () noexcept {
    auto l2 = avx_vdpps<0x33, f64>(p, p);
    auto l = avx_vsqrtps<f64>(l2);
    p = avx_vdivps<f32>(p, l);
    return avx_cast<f32>(l);
  }


  inline Vec2 operator+ (Vec2 v) const noexcept {
    return avx_vaddps<f64>(p, v.p);
  }
  inline Vec2 operator- (Vec2 v) const noexcept {
    return avx_vsubps<f64>(p, v.p);
  }
  inline Vec2 operator* (Vec2 v) const noexcept {
    return avx_vmulps<f64>(p, v.p);
  }
  inline Vec2 operator/ (Vec2 v) const noexcept {
    return avx_vdivps<f64>(p, v.p);
  }


  inline Vec2 operator* (f32 s) const noexcept {
    auto s2 = avx_vshufps<0x00, f64>(s, s);
    return avx_vmulps<f64>(p, s2);
  }
  inline Vec2 operator/ (f32 s) const noexcept {
    auto s2 = avx_vshufps<0x00, f64>(s, s);
    return avx_vdivps<f64>(p, s2);
  }


  inline f32 operator& (Vec2 v) const noexcept {
    return avx_vdpps<0x31, f32>(p, v.p);
  }
  inline f32 operator^ (Vec2 v) const noexcept {
    auto p2 = avx_vshufps<0x01, f64>(v.p, v.p);
    auto r2 = avx_vmulps<f64>(p, p2);
    return avx_vhsubps<f32>(r2, r2);
  }


  inline Vec2 operator+ () const noexcept {
    f64 t;
    auto s = avx_vpcmpeqq<f64>(t, t);
    auto m = avx_vpsrld<1, f64>(s);
    return avx_vandps<f64>(m, p);
  }
  inline Vec2 operator- () const noexcept {
    f64 t;
    auto z = avx_vxorps<f64>(t, t);
    return avx_vsubps<f64>(z, p);
  }
  inline Vec2 operator* () const noexcept {
    auto l2 = avx_vdpps<0x33, f64>(p, p);
    auto l = avx_vsqrtps<f64>(l2);
    return avx_vdivps<f64>(p, l);
  }
  inline f32 operator~ () const noexcept {
    auto l2 = avx_vdpps<0x31, f32>(p, p);
    return avx_vsqrtss<f32>(l2, l2);
  }


  inline static Vec2 zero () noexcept {
    f64 t;
    return avx_vxorps<f64>(t, t);
  }
  inline static Vec2 one () noexcept {
    return avx_vshufps<0xE0, f64>(1.0F, 1.0F);
  }
  template<u8 Index>
  inline static Vec2 unit () noexcept {
    return avx_vinsertps<Index | (0xF^(1<<Index)), f64>(1.0F, 1.0F);
  }


  inline static f32 distance (Vec2 a, Vec2 b) noexcept {
    return ~(b-a);
  }
  inline static Vec2 min (Vec2 a, Vec2 b) noexcept {
    return avx_vminps<f64>(a.p, b.p);
  }
  inline static Vec2 max (Vec2 a, Vec2 b) noexcept {
    return avx_vmaxps<f64>(a.p, b.p);
  }
  inline static Vec2 clamp (Vec2 v, Vec2 min, Vec2 max) noexcept {
    return Vec2::min(Vec2::max(v, min), max);
  }
  inline static Vec2 lerp (Vec2 a, Vec2 b, Vec2 t) noexcept {
    return a + (b-a)*t;
  }
  inline static Vec2 slerp (Vec2 a, Vec2 b, f32 t) noexcept {
    auto La = a.normalize();
    auto Lb = b.normalize();
    auto Lt = La + (Lb-La)*t;
    auto cp = a ^ b;
    auto R = asinf(cp);
    auto r = R * t;
    auto p = sinf(R-r) / cp;
    auto q = sinf(r) / cp;
    return (a*p + b*q) * Lt;
  }
};


inline Vec2 operator* (f32 a, Vec2 b) noexcept {
  return b * a;
}


CLOSE_JLIB_NS
